#include "gpu/gpu2D.h"
#include "math/geometry.h"

/**
 * Init Engine 2D
 */
void gpu2D_Init (void) {

}

/**
 * Lines primitives
 */
void gpu2D_DrawHLine (Line *line, unsigned int color) {
	unsigned char x0,x1,y;
	unsigned char aux;
	unsigned int aux1,aux2;
	int aux0;

	x0 = line->p1.x;
	x1 = line->p2.x;
	y  = line->p1.y;

	aux = (x0 <= x1) ? x0 : x1;
	x1 = (x0 > x1) ? x0 : x1;

	spilcd_SendCommand(PASETP);
	spilcd_SendData(aux);
	spilcd_SendData(x1);
	// Column address set (command 0x2A)
	spilcd_SendCommand(CASETP);
	spilcd_SendData(y);
	spilcd_SendData(y);
	// WRITE MEMORY
	spilcd_SendCommand(RAMWRP);

	#ifdef LCD_COLOR_DEPTH_16
	while (aux < x1) {
			spilcd_SendData((unsigned char)(color>>8));
			spilcd_SendData((unsigned char)(color & 0x00FF));
		aux++;
	}
	#endif

	#ifdef LCD_COLOR_DEPTH_12
		aux0 = x1 - aux +1;
		aux0 = aux0 /2;
		while (aux0 >= 0) {
			spilcd_SendData((unsigned char)(color>>4)&0x00FF);
			spilcd_SendData((unsigned char)((color&0x0F)<<4)|((color>>8) & 0x0F));
			spilcd_SendData((unsigned char)(color&0x0FF));
			aux0--;
		}

		aux0 = x1 - aux +1;
		if ((aux0 % 2) != 0) {
			spilcd_SendData((unsigned char)(color>>4)&0x00FF);
			spilcd_SendData((unsigned char)((color&0x0F)<<4)|(0x00));
			spilcd_SendCommand(NOP_NXP);
		}
	#endif

	#ifdef LCD_COLOR_DEPTH_8
		while(aux < x1) {
			spilcd_SendData((unsigned char)color);
		}
	#endif
}

void gpu2D_DrawVLine (Line *line, unsigned int color) {
	unsigned char y0,y1,x;
	unsigned char aux;
	int aux0;

	y0 = line->p1.y;
	y1 = line->p2.y;
	x  = line->p1.x;

	aux = (y0 <= y1) ? y0 : y1;
	y1 = (y0 > y1) ? y0 : y1;

	spilcd_SendCommand(PASETP);
	spilcd_SendData(x);
	spilcd_SendData(x);
	// Column address set (command 0x2A)
	spilcd_SendCommand(CASETP);
	spilcd_SendData(aux);
	spilcd_SendData(y1);
	// WRITE MEMORY
	spilcd_SendCommand(RAMWRP);

	#ifdef LCD_COLOR_DEPTH_16
	while (aux < y1) {
			spilcd_SendData((unsigned char)(color>>8));
			spilcd_SendData((unsigned char)(color & 0x00FF));
		aux++;
	}
	#endif

	#ifdef LCD_COLOR_DEPTH_12
		aux0 = y1 - aux +1;
		aux0 = aux0 /2;
		while (aux0 >= 0) {
			spilcd_SendData((unsigned char)(color>>4)&0x00FF);
			spilcd_SendData((unsigned char)((color&0x0F)<<4)|((color>>8) & 0x0F));
			spilcd_SendData((unsigned char)(color&0x0FF));
			aux0--;
		}

		aux0 = y1 - aux +1;
		if ((aux0 % 2) != 0) {
			spilcd_SendData((unsigned char)(color>>4)&0x00FF);
			spilcd_SendData((unsigned char)((color&0x0F)<<4)|(0x00));
			spilcd_SendCommand(NOP_NXP);
		}
	#endif

	#ifdef LCD_COLOR_DEPTH_8
		while(aux < y1) {
			spilcd_SendData((unsigned char)color);
		}
	#endif
}

void gpu2D_DrawLine2 (int x1, int y1, int x2, int y2 , unsigned int color) {
	int dy;
	int dx;
	int StepX, StepY;
	int Fraction;

	dy = y2 - y1;
	dx = x2 - x1;

	if (dy < 0) {
		dy = -dy;
		StepY = -1;
	}
	else
		StepY = 1;

	if (dx < 0) {
		dx = -dx;
		StepX = -1;
	}
	else
		StepX = 1;

	dy <<= 1; 									// dy is now 2*dy
	dx <<= 1; 									// dx is now 2*dx

	spilcd_PutPixel(color,x1,y1);

	if (dx > dy) {
		Fraction = dy - (dx >> 1); 				// same as 2*dy - dx
		while (x1 != x2) {
			if (Fraction >= 0) {
				y1 += StepY;
				Fraction -= dx; 				// same as fraction -= 2*dx
			}

			x1 += StepX;
			Fraction += dy; 					// same as fraction -= 2*dy
			spilcd_PutPixel(color,x1,y1);
		}
	}

	else {
		Fraction = dx - (dy >> 1);
		while (y1 != y2) {
			if (Fraction >= 0) {
				x1 += StepX;
				Fraction -= dy;
			}

			y1 += StepY;
			Fraction += dx;
			spilcd_PutPixel(color,x1,y1);
		}
	}
}

void gpu2D_DrawLine  (Line *line, unsigned int color) {
	if (line->p1.x == line->p2.x) {
		gpu2D_DrawVLine(line,color);
	}
	else if (line->p1.y == line->p2.y) {
		gpu2D_DrawHLine(line,color);
	}
	else {
		gpu2D_DrawLine2(line->p1.x,line->p1.y,line->p2.x,line->p2.y,color);
	}
/*		int dy;
		int dx;
		int StepX, StepY;
		int Fraction;
		unsigned char x1,x2,y1,y2;


		y1 = line->p1.y;
		y2 = line->p2.y;
		x1 = line->p1.x;
		x2 = line->p2.x;

		dy = y2 - y1;
		dx = x2 - x1;

		if (dy < 0) {
			dy = -dy;
			StepY = -1;
		}
		else
			StepY = 1;

		if (dx < 0) {
			dx = -dx;
			StepX = -1;
		}
		else
			StepX = 1;

		dy <<= 1; 									// dy is now 2*dy
		dx <<= 1; 									// dx is now 2*dx

		spilcd_PutPixel(color,x1,y1);

		if (dx > dy) {
			Fraction = dy - (dx >> 1); 				// same as 2*dy - dx
			while (x1 != x2) {
				if (Fraction >= 0) {
					y1 += StepY;
					Fraction -= dx; 				// same as fraction -= 2*dx
				}

				x1 += StepX;
				Fraction += dy; 					// same as fraction -= 2*dy
				spilcd_PutPixel(color,x1,y1);
			}
		}

		else {
			Fraction = dx - (dy >> 1);
			while (y1 != y2) {
				if (Fraction >= 0) {
					x1 += StepX;
					Fraction -= dy;
				}

				y1 += StepY;
				Fraction += dx;
				spilcd_PutPixel(color,x1,y1);
			}
		}
	}
	*/
}


/**
 * Boxes primitives
 */
void gpu2D_DrawRect (Rect *rect, unsigned int color) {
	Line line;

	line.p1.x = rect->p1.x;
	line.p1.y = rect->p1.y;
	line.p2.x = rect->p2.x;
	line.p2.y = rect->p1.y;
	gpu2D_DrawHLine(&line,color);

	line.p1.x = rect->p1.x;
	line.p1.y = rect->p2.y;
	line.p2.x = rect->p2.x;
	line.p2.y = rect->p2.y;
	gpu2D_DrawHLine(&line,color);


	line.p1.x = rect->p1.x;
	line.p1.y = rect->p1.y;
	line.p2.x = rect->p1.x;
	line.p2.y = rect->p2.y;
	gpu2D_DrawVLine(&line,color);

	line.p1.x = rect->p2.x;
	line.p1.y = rect->p1.y;
	line.p2.x = rect->p2.x;
	line.p2.y = rect->p2.y;
	gpu2D_DrawVLine(&line,color);
}

void gpu2D_FillRect (Rect *rect, unsigned int color) {
	unsigned int xmin, xmax, ymin, ymax;
	unsigned int i,j,qty;
	// check if the rectangle is to be filled

	// best way to create a filled rectangle is to define a drawing box
	// and loop two pixels at a time
	// calculate the min and max for x and y directions
	xmin = (rect->p1.x <= rect->p2.x) ? rect->p1.x : rect->p2.x;
	xmax = (rect->p1.x > rect->p2.x) ? rect->p1.x : rect->p2.x;
	ymin = (rect->p1.y <= rect->p2.y) ? rect->p1.y : rect->p2.y;
	ymax = (rect->p1.y > rect->p2.y) ? rect->p1.y : rect->p2.y;

	spilcd_SendCommand(PASETP);
	spilcd_SendData(xmin);
	spilcd_SendData(xmax);
	// Column address set (command 0x2A)
	spilcd_SendCommand(CASETP);
	spilcd_SendData(ymin);
	spilcd_SendData(ymax);
	// WRITE MEMORY
	spilcd_SendCommand(RAMWRP);

	#ifdef LCD_COLOR_DEPTH_16
	for (i = 0; i < ((((xmax - xmin + 1) * (ymax - ymin + 1)))); i++) {
			spilcd_SendData((unsigned char)(color>>8));
			spilcd_SendData((unsigned char)(color & 0x00FF));

	}
	#endif

	#ifdef LCD_COLOR_DEPTH_12
	qty = ((((xmax - xmin + 1) * (ymax - ymin + 1)))) /2;
	for (i = 0; i < qty; i++) {
			spilcd_SendData((unsigned char)(color>>4)&0x00FF);
			spilcd_SendData((unsigned char)((color&0x0F)<<4)|((color>>8) & 0x0F));
			spilcd_SendData((unsigned char)(color&0x0FF));
	}
	#endif

	#ifdef LCD_COLOR_DEPTH_8

	#endif
}


/**
 * Circles primitives
 */
void gpu2D_DrawCircle (Circle * circle, unsigned int color)  {
	int f;
	int ddF_x;
	int ddF_y;
	int x;
	int y;
	Point p;
	unsigned char x0,y0;

	x0 = circle->center.x;
	y0 = circle->center.y;

	f = 1 - circle->radius;
	ddF_x = 0;
	ddF_y = -2 * circle->radius;
	x = 0;
	y = circle->radius;

	p.x = x0;
	p.y = y0 + circle->radius;
	lcd_DrawPoint(&p, color);

	p.x = x0;
	p.y = y0 - circle->radius;
	lcd_DrawPoint(&p, color);

	p.x = x0 + circle->radius;
	p.y = y0;
	lcd_DrawPoint(&p, color);

	p.x = x0 - circle->radius;
	p.y = y0;
	lcd_DrawPoint(&p, color);

	while (x < y)
	{	if (f >= 0)
		{	y--;
			ddF_y += 2;
			f += ddF_y;
		}

		x++;
		ddF_x += 2;
		f += ddF_x + 1;

		p.x = x0 + x;
		p.y = y0 + y;
		lcd_DrawPoint(&p, color);

		p.x = x0 - x;
		p.y = y0 + y;
		lcd_DrawPoint(&p, color);

		p.x = x0 + x;
		p.y = y0 - y;
		lcd_DrawPoint(&p, color);

		p.x = x0 - x;
		p.y = y0 - y;
		lcd_DrawPoint(&p, color);

		p.x = x0 + y;
		p.y = y0 + x;
		lcd_DrawPoint(&p, color);

		p.x = x0 - y;
		p.y = y0 + x;
		lcd_DrawPoint(&p, color);

		p.x = x0 + y;
		p.y = y0 - x;
		lcd_DrawPoint(&p, color);

		p.x = x0 - y;
		p.y = y0 - x;
		lcd_DrawPoint(&p, color);
	}
}

void gpu2D_FillCircle (Circle * circle, unsigned int color) {
	int f;
	int ddF_x;
	int ddF_y;
	int x;
	int y;
	unsigned char x0,y0;
	Line line;

	x0 = circle->center.x;
	y0 = circle->center.y;

	f = 1 - circle->radius;
	ddF_x = 0;
	ddF_y = -2 * circle->radius;
	x = 0;
	y = circle->radius;

	line.p1.x = x0;
	line.p1.y = y0 + circle->radius;
	line.p2.x = x0;
	line.p2.y = y0 - circle->radius;
	gpu2D_DrawVLine(&line, color);

	line.p1.x = x0 + circle->radius;
	line.p1.y = y0;
	line.p2.x = x0 - circle->radius;
	line.p2.y = y0;
	gpu2D_DrawHLine(&line, color);

	while (x < y)
	{	if (f >= 0)
		{	y--;
			ddF_y += 2;
			f += ddF_y;
		}

		x++;
		ddF_x += 2;
		f += ddF_x + 1;

		line.p1.x = x0 + x;
		line.p1.y = y0 + y;
		line.p2.x = x0 - x;
		line.p2.y = y0 + y;
		gpu2D_DrawHLine(&line, color);

		line.p1.x = x0 + x;
		line.p1.y = y0 - y;
		line.p2.x = x0 - x;
		line.p2.y = y0 - y;
		gpu2D_DrawHLine(&line, color);

		line.p1.x = x0 + y;
		line.p1.y = y0 + x;
		line.p2.x = x0 - y;
		line.p2.y = y0 + x;
		gpu2D_DrawHLine(&line, color);

		line.p1.x = x0 + y;
		line.p1.y = y0 - x;
		line.p2.x = x0 - y;
		line.p2.y = y0 - x;
		gpu2D_DrawHLine(&line, color);
	}
}

/**
 * Triangles primitives
 */
void gpu2D_DrawTriangle (Triangle *triangle, unsigned int color) {
	Line line;

	line.p1.x = triangle->p1.x;
	line.p1.y = triangle->p1.y;
	line.p2.x = triangle->p2.x;
	line.p2.y = triangle->p2.y;
	gpu2D_DrawLine(&line, color);

	line.p1.x = triangle->p1.x;
	line.p1.y = triangle->p1.y;
	line.p2.x = triangle->p3.x;
	line.p2.y = triangle->p3.y;
	gpu2D_DrawLine(&line, color);

	line.p1.x = triangle->p2.x;
	line.p1.y = triangle->p2.y;
	line.p2.x = triangle->p3.x;
	line.p2.y = triangle->p3.y;
	gpu2D_DrawLine(&line, color);
}

void gpu2D_FillTriangle (Triangle *triangle, unsigned int color) {
	float sx,sy,ex,ey;
	float dx1,dx2,dx3,aux;
	Point * a, *b, *c;
	Line line;

	if (triangle->p1.y <= triangle->p2.y && triangle->p1.y <= triangle->p3.y) {
		a = &(triangle->p1);
		if (triangle->p2.y <= triangle->p3.y) {
			b = &(triangle->p2);
			c = &(triangle->p3);
		}
		else {
			b = &(triangle->p3);
			c = &(triangle->p2);
		}
	}
	else if (triangle->p2.y <= triangle->p1.y && triangle->p2.y <= triangle->p3.y) {
		a = &(triangle->p2);
		if (triangle->p1.y <= triangle->p3.y) {
			b = &(triangle->p1);
			c = &(triangle->p3);
		}
		else {
			b = &(triangle->p3);
			c = &(triangle->p1);
		}
	}
	else if (triangle->p3.y <= triangle->p1.y && triangle->p3.y <= triangle->p2.y) {
		a = &(triangle->p3);
		if (triangle->p1.y <= triangle->p2.y) {
			b = &(triangle->p1);
			c = &(triangle->p2);
		}
		else {
			b = &(triangle->p2);
			c = &(triangle->p1);
		}
	}

	dx1 = 0;
	if (b->y > a->y) {
		dx1 = (float)(b->x);
		dx1 -= (float)a->x;
		aux = (float)b->y;
		aux -= (float)a->y;
		dx1 /= aux;
	}

	dx2 = 0;
	if (c->y > a->y) {
		dx2 = (float)(c->x);
		dx2 -= (float)a->x;
		aux = (float)c->y;
		aux -= (float)a->y;
		dx2 /= aux;
	}

	dx3 = 0;
	if (c->y > b->y) {
		dx3 = (float)(c->x);
		dx3 -= (float)b->x;
		aux = (float)c->y;
		aux -= (float)b->y;
		dx3 /= aux;
	}

	sx = (float)a->x;
	ex = sx;
	sy = (float)a->y;
	ey = sy;


	if(dx1 > dx2) {
		for(;sy<=b->y;sy++,ey++,sx+=dx2,ex+=dx1) {
			line.p1.x = (unsigned char)sx;
			line.p1.y = (unsigned char)sy;
			line.p2.x = (unsigned char)ex;
			line.p2.y = (unsigned char)sy;
			gpu2D_DrawHLine(&line,color);
		}
		ex = b->x;
		ey = b->y;
		for(;sy<=c->y;sy++,ey++,sx+=dx2,ex+=dx3) {
			line.p1.x = (unsigned char)sx;
			line.p1.y = (unsigned char)sy;
			line.p2.x = (unsigned char)ex;
			line.p2.y = (unsigned char)sy;
			gpu2D_DrawHLine(&line,color);
		}
	} else {
		for(;sy<=b->y;sy++,ey++,sx+=dx1,ex+=dx2) {
			line.p1.x = (unsigned char)sx;
			line.p1.y = (unsigned char)sy;
			line.p2.x = (unsigned char)ex;
			line.p2.y = (unsigned char)sy;
			gpu2D_DrawHLine(&line,color);
		}

		sx = b->x;
		sy = b->y;
		for(;sy<=c->y;sy++,ey++,sx+=dx3,ex+=dx2) {
			line.p1.x = (unsigned char)sx;
			line.p1.y = (unsigned char)sy;
			line.p2.x = (unsigned char)ex;
			line.p2.y = (unsigned char)sy;
			gpu2D_DrawHLine(&line,color);
		}
	}

	gpu2D_DrawTriangle(triangle,color);
}
